# AprilCalib log 3
# CalibRig::mode=2d
# @ Mon Nov 25 12:08:13 2019

from numpy import array
U=array([[167.5771484375, 257.5049438476563, 211.3677062988281, 305.7357177734375, 353.63134765625, 400.5049438476563, 444.0433654785156, 484.37158203125, 158.0724487304688, 203.2400817871094, 302.17041015625, 251.6460266113281, 401.8928833007813, 352.8693542480469, 447.8633422851563, 489.6557006835938, 195.3932342529297, 148.6904296875, 298.6182861328125, 351.5572509765625, 245.8951873779297, 402.8372802734375, 450.4790954589844, 493.5982055664063, 188.3005523681641, 140.4792022705078, 349.5315856933594, 294.7433166503906, 402.4978942871094, 451.6826782226563, 240.2535095214844, 495.9717407226563, 182.2177886962891, 133.722412109375, 235.0336456298828, 290.55859375, 400.8753356933594, 496.0656433105469, 346.6632995605469, 451.076416015625],
       [132.0372772216797, 132.4173431396484, 131.3936614990234, 134.5692749023438, 137.8582611083984, 142.2270050048828, 147.3482055664063, 153.5367584228516, 172.0995635986328, 172.8159027099609, 176.7054443359375, 174.1874237060547, 184.1888580322266, 180.3278656005859, 188.8742370605469, 193.8572387695313, 218.7190399169922, 216.5401153564453, 224.287353515625, 227.6983642578125, 221.1506958007813, 231.2054595947266, 235.0069885253906, 238.5790252685547, 268.4794921875, 264.7607116699219, 279.1387634277344, 275.8332214355469, 281.9884643554688, 284.3514404296875, 272.3122253417969, 286.3221435546875, 320.8125305175781, 314.9626159667969, 325.9285888671875, 330.2201538085938, 335.3537902832031, 336.0873718261719, 333.4665222167969, 336.2279357910156]], dtype='float64');
Xw=array([[99.50446319580078, 499.5044555664063, 299.5044555664063, 699.5044555664063, 899.5044555664063, 1099.504516601563, 1299.504516601563, 1499.504516601563, 99.50446319580078, 299.5044555664063, 699.5044555664063, 499.5044555664063, 1099.504516601563, 899.5044555664063, 1299.504516601563, 1499.504516601563, 299.5044555664063, 99.50446319580078, 699.5044555664063, 899.5044555664063, 499.5044555664063, 1099.504516601563, 1299.504516601563, 1499.504516601563, 299.5044555664063, 99.50446319580078, 899.5044555664063, 699.5044555664063, 1099.504516601563, 1299.504516601563, 499.5044555664063, 1499.504516601563, 299.5044555664063, 99.50446319580078, 499.5044555664063, 699.5044555664063, 1099.504516601563, 1499.504516601563, 899.5044555664063, 1299.504516601563],
       [99.50446319580078, 99.50446319580078, 99.50446319580078, 99.50446319580078, 99.50446319580078, 99.50446319580078, 99.50446319580078, 99.50446319580078, 299.5044555664063, 299.5044555664063, 299.5044555664063, 299.5044555664063, 299.5044555664063, 299.5044555664063, 299.5044555664063, 299.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063],
       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype='float64');
# After LM:
K=array([[402.1104294442784, 0, 310.8077302132485],
       [0, 400.3516436916167, 259.8945437011783],
       [0, 0, 1]], dtype='float64');
distCoeffs=array([-0.3471318843905968,
       0.1061706433621047,
       -0.007768383606796321,
       -0.008577063657929599,
       0], dtype='float64');
CovK=array([[1.363844443743121, 1.391031732609459, 0.06584690357694682, -0.09477624607412528, -0.002496343831100889, 0.002024399780625103, 6.177505849351458e-05, -4.17325910290892e-05, -0.0003855211461820527],
       [1.391031732609485, 1.41999334943845, 0.07082490887066821, -0.09457736676423124, -0.002540522762727362, 0.002049759509163503, 6.250475111124506e-05, -4.417956867643178e-05, -0.0003864772499275291],
       [0.06584690357780611, 0.07082490887154703, 0.0715172364595187, 0.002623532554370046, -0.0001464424754615013, 0.0002236362117943676, 7.153601607203204e-06, -2.886575889385921e-05, -0.0002004633630479598],
       [-0.09477624607458499, -0.09457736676470228, 0.002623532554318783, 0.05608738857847458, 0.0001636910131660865, -0.0001335940997486434, -2.822646963743854e-05, -2.739491749572235e-06, -2.422787419053699e-05],
       [-0.002496343831101353, -0.002540522762727797, -0.0001464424754599015, 0.0001636910131651392, 6.45550789882329e-06, -1.047035839171241e-05, -1.072275264854955e-07, 9.112832766412489e-08, 7.228374924548382e-06],
       [0.002024399780627413, 0.002049759509165909, 0.0002236362117930409, -0.0001335940997475979, -1.047035839171502e-05, 2.925022261183348e-05, 1.15487617899245e-07, -1.700660293226567e-07, -2.698625738182319e-05],
       [6.177505849383019e-05, 6.250475111157574e-05, 7.153601607225606e-06, -2.822646963742328e-05, -1.072275264861118e-07, 1.154876178998406e-07, 2.188800518857699e-08, -2.293672060279744e-09, -2.221266488640357e-08],
       [-4.173259102942289e-05, -4.417956867677316e-05, -2.886575889384199e-05, -2.739491749592913e-06, 9.112832766473457e-08, -1.700660293231271e-07, -2.293672060267932e-09, 1.805027649487272e-08, 1.734904009347014e-07],
       [-0.0003855211461846903, -0.0003864772499303308, -0.0002004633630475824, -2.422787419103033e-05, 7.228374924552096e-06, -2.698625738182601e-05, -2.221266488615982e-08, 1.734904009346121e-07, 2.774542253993575e-05]], dtype='float64');
# rms=0.244279
r0=array([0.1525014281775149,
       0.1477661673936254,
       -1.954805778776299], dtype='float64');
t0=array([-611.5427109038064,
       673.0798661569119,
       1866.816232113207], dtype='float64');
Covr0=array([[7.752239729457234e-07, 1.320547222640963e-07, 5.939979169049921e-08],
       [1.320547222634682e-07, 6.023050031271209e-07, 7.300726494269798e-08],
       [5.939979169055489e-08, 7.300726494281503e-08, 2.040802864316904e-08]], dtype='float64');
Covt0=array([[1.541079482302274, 0.1168700378591434, -1.372790348730142],
       [0.1168700378577906, 1.086793372213761, 1.24569700859602],
       [-1.372790348711663, 1.245697008586874, 28.57319658400912]], dtype='float64');
r1=array([-0.1586987006503196,
       0.07788661283367794,
       -0.03111536388242278], dtype='float64');
t1=array([-617.4816357289042,
       -615.6836675290609,
       1691.441110893126], dtype='float64');
Covr1=array([[4.115238864766381e-07, -5.659514112850187e-08, -5.855458631093682e-09],
       [-5.659514112813756e-08, 2.387272027825233e-07, 3.06643537544676e-08],
       [-5.855458631029963e-09, 3.066435375444203e-08, 8.753948846513208e-09]], dtype='float64');
Covt1=array([[1.229448898831505, 0.01595381394793182, -0.7434447672950218],
       [0.01595381394725661, 0.9575331762113211, 1.950211678370357],
       [-0.7434447672803699, 1.950211678362812, 23.15075676431187]], dtype='float64');
r2=array([-0.1065995112870453,
       -0.04394153105952934,
       0.6878908438037133], dtype='float64');
t2=array([-113.1398331191611,
       -1168.815525183474,
       1547.826634797551], dtype='float64');
Covr2=array([[3.231559594278534e-07, 7.574960935250276e-08, 1.398985023764214e-10],
       [7.57496093525577e-08, 3.277397455553699e-07, 4.221598787557786e-08],
       [1.398985023750485e-10, 4.221598787555899e-08, 9.503650034389443e-09]], dtype='float64');
Covt2=array([[1.13707914347502, 0.03428797824706725, -0.9815611900673794],
       [0.03428797824629853, 0.8252876098635266, 1.66146069309417],
       [-0.9815611900546906, 1.661460693087759, 20.61237907985612]], dtype='float64');
r3=array([-0.2626221546105175,
       0.007550462226963077,
       0.06365676325015192], dtype='float64');
t3=array([-712.2378105829617,
       -659.0661620368468,
       1651.600414467508], dtype='float64');
Covr3=array([[5.593671125219012e-07, 5.18525901773995e-08, 6.397138627817314e-09],
       [5.185259017785205e-08, 2.391050806812173e-07, 4.221302215748581e-08],
       [6.397138627922914e-09, 4.22130221574786e-08, 1.165306192430863e-08]], dtype='float64');
Covt3=array([[1.180475017789789, -0.001471499201849694, -0.7919381928363197],
       [-0.001471499202420215, 0.9305098394670354, 2.042781250653178],
       [-0.791938192822748, 2.042781250646163, 21.34451667762107]], dtype='float64');
